﻿using System;

namespace DroneTargetLocator
{
    public class CameraModel
    {
        public double FocalLengthX { get; }    // 像素单位的焦距 (fx)
        public double FocalLengthY { get; }    // 像素单位的焦距 (fy)
        public double PrincipalPointX { get; } // 主点 x 坐标 (cx)
        public double PrincipalPointY { get; } // 主点 y 坐标 (cy)
        public double ImageWidth { get; }      // 图像宽度 (像素)
        public double ImageHeight { get; }     // 图像高度 (像素)

        public CameraModel(double focalLengthMm, double pixelSizeUm, double width, double height)
        {
            // 计算像素单位的焦距
            double focalLengthPixels = focalLengthMm * 1000 / pixelSizeUm;
            FocalLengthX = focalLengthPixels;
            FocalLengthY = focalLengthPixels;

            // 主点设为图像中心
            PrincipalPointX = width / 2.0;
            PrincipalPointY = height / 2.0;

            ImageWidth = width;
            ImageHeight = height;
        }

        /// <summary>
        /// 将图像坐标转换为相机坐标系中的归一化方向向量
        /// </summary>
        public (double X, double Y, double Z) ImageToCameraVector(double px, double py)
        {
            // 转换为归一化图像坐标
            double nx = (px - PrincipalPointX) / FocalLengthX;
            double ny = (py - PrincipalPointY) / FocalLengthY;

            // 归一化方向向量
            double length = Math.Sqrt(nx * nx + ny * ny + 1);
            return (nx / length, ny / length, -1 / length);
        }
    }

    public static class GeodeticUtils
    {
        // WGS84 椭球参数
        private const double EquatorialRadius = 6378137.0;
        private const double Flattening = 1.0 / 298.257223563;
        private const double EccentricitySquared = 2 * Flattening - Flattening * Flattening;

        /// <summary>
        /// 高精度将经纬度转换为ECEF坐标 (WGS84)
        /// </summary>
        public static (double X, double Y, double Z) GeodeticToEcef(double lat, double lon, double alt)
        {
            double latRad = DegreeToRadian(lat);
            double lonRad = DegreeToRadian(lon);

            double n = EquatorialRadius / Math.Sqrt(1 - EccentricitySquared * Math.Sin(latRad) * Math.Sin(latRad));
            double x = (n + alt) * Math.Cos(latRad) * Math.Cos(lonRad);
            double y = (n + alt) * Math.Cos(latRad) * Math.Sin(lonRad);
            double z = (n * (1 - EccentricitySquared) + alt) * Math.Sin(latRad);

            return (x, y, z);
        }

        /// <summary>
        /// 高精度将ECEF坐标转换为经纬度
        /// </summary>
        public static (double Lat, double Lon, double Alt) EcefToGeodetic(double x, double y, double z)
        {
            const double tolerance = 1e-12;
            const int maxIterations = 100;

            double p = Math.Sqrt(x * x + y * y);
            double lon = Math.Atan2(y, x);

            // 初始估计
            double lat = Math.Atan2(z, p * (1.0 - EccentricitySquared));
            double alt = 0;
            double n = EquatorialRadius;

            for (int i = 0; i < maxIterations; i++)
            {
                double sinLat = Math.Sin(lat);
                double prevLat = lat;

                n = EquatorialRadius / Math.Sqrt(1 - EccentricitySquared * sinLat * sinLat);
                alt = p / Math.Cos(lat) - n;

                lat = Math.Atan2(z + n * EccentricitySquared * sinLat, p);

                if (Math.Abs(lat - prevLat) < tolerance) break;
            }

            return (RadianToDegree(lat), RadianToDegree(lon), alt);
        }

        /// <summary>
        /// 高精度将ENU坐标转换为ECEF坐标
        /// </summary>
        public static (double X, double Y, double Z) EnuToEcef(
            double east, double north, double up,
            double refLat, double refLon, double refAlt)
        {
            (double refX, double refY, double refZ) = GeodeticToEcef(refLat, refLon, refAlt);

            double latRad = DegreeToRadian(refLat);
            double lonRad = DegreeToRadian(refLon);

            double sinLat = Math.Sin(latRad);
            double cosLat = Math.Cos(latRad);
            double sinLon = Math.Sin(lonRad);
            double cosLon = Math.Cos(lonRad);

            // 旋转矩阵元素
            double t11 = -sinLon;
            double t12 = -sinLat * cosLon;
            double t13 = cosLat * cosLon;

            double t21 = cosLon;
            double t22 = -sinLat * sinLon;
            double t23 = cosLat * sinLon;

            double t31 = 0;
            double t32 = cosLat;
            double t33 = sinLat;

            double dx = t11 * east + t12 * north + t13 * up;
            double dy = t21 * east + t22 * north + t23 * up;
            double dz = t31 * east + t32 * north + t33 * up;

            return (refX + dx, refY + dy, refZ + dz);
        }

        /// <summary>
        /// 构建高精度旋转矩阵 (使用四元数避免万向节锁)
        /// </summary>
        public static double[,] CreateRotationMatrix(double yawDeg, double pitchDeg, double rollDeg)
        {
            double yaw = DegreeToRadian(NormalizeAngle(yawDeg));
            double pitch = DegreeToRadian(NormalizeAngle(pitchDeg));
            double roll = DegreeToRadian(NormalizeAngle(rollDeg));

            // 使用四元数避免万向节锁
            double cy = Math.Cos(yaw * 0.5);
            double sy = Math.Sin(yaw * 0.5);
            double cp = Math.Cos(pitch * 0.5);
            double sp = Math.Sin(pitch * 0.5);
            double cr = Math.Cos(roll * 0.5);
            double sr = Math.Sin(roll * 0.5);

            // 四元数
            double w = cr * cp * cy + sr * sp * sy;
            double x = sr * cp * cy - cr * sp * sy;
            double y = cr * sp * cy + sr * cp * sy;
            double z = cr * cp * sy - sr * sp * cy;

            // 从四元数转换为旋转矩阵
            double xx = x * x;
            double xy = x * y;
            double xz = x * z;
            double xw = x * w;

            double yy = y * y;
            double yz = y * z;
            double yw = y * w;

            double zz = z * z;
            double zw = z * w;

            return new double[,]
            {
                { 1 - 2 * (yy + zz),     2 * (xy - zw),     2 * (xz + yw) },
                {     2 * (xy + zw), 1 - 2 * (xx + zz),     2 * (yz - xw) },
                {     2 * (xz - yw),     2 * (yz + xw), 1 - 2 * (xx + yy) }
            };
        }

        /// <summary>
        /// 计算两点间的大地距离 (Vincenty公式)
        /// </summary>
        public static double CalculateGeodeticDistance(double lat1, double lon1, double lat2, double lon2)
        {
            const double a = EquatorialRadius;
            const double b = a * (1 - Flattening);
            const double f = Flattening;

            double L = DegreeToRadian(lon2 - lon1);
            double U1 = Math.Atan((1 - f) * Math.Tan(DegreeToRadian(lat1)));
            double U2 = Math.Atan((1 - f) * Math.Tan(DegreeToRadian(lat2)));

            double sinU1 = Math.Sin(U1);
            double cosU1 = Math.Cos(U1);
            double sinU2 = Math.Sin(U2);
            double cosU2 = Math.Cos(U2);

            double lambda = L;
            double lambdaP = 2 * Math.PI;
            double sinSigma = 0;
            double cosSigma = 0;
            double sigma = 0;
            double cosSqAlpha = 0;
            double cos2SigmaM = 0;

            int iterLimit = 100;
            double sinLambda, cosLambda;

            do
            {
                sinLambda = Math.Sin(lambda);
                cosLambda = Math.Cos(lambda);

                sinSigma = Math.Sqrt(
                    Math.Pow(cosU2 * sinLambda, 2) +
                    Math.Pow(cosU1 * sinU2 - sinU1 * cosU2 * cosLambda, 2)
                );

                if (Math.Abs(sinSigma) < 1e-12) return 0;  // 重合点

                cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
                sigma = Math.Atan2(sinSigma, cosSigma);

                double sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma;
                cosSqAlpha = 1 - sinAlpha * sinAlpha;

                if (Math.Abs(cosSqAlpha) < 1e-12)
                {
                    cos2SigmaM = 0;
                }
                else
                {
                    cos2SigmaM = cosSigma - 2 * sinU1 * sinU2 / cosSqAlpha;
                }

                double C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha));
                lambdaP = lambda;
                lambda = L + (1 - C) * f * sinAlpha *
                    (sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)));

            } while (Math.Abs(lambda - lambdaP) > 1e-12 && --iterLimit > 0);

            if (iterLimit == 0) return double.NaN;  // 公式未收敛

            double uSq = cosSqAlpha * (a * a - b * b) / (b * b);
            double A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
            double B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));

            double deltaSigma = B * sinSigma *
                (cos2SigmaM + B / 4 *
                 (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) -
                  B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) * (-3 + 4 * cos2SigmaM * cos2SigmaM)));

            double s = b * A * (sigma - deltaSigma);

            return s;
        }

        /// <summary>
        /// 计算从起点到终点的方位角（正北为0°，顺时针增加）
        /// </summary>
        public static double CalculateAzimuth(double lat1, double lon1, double lat2, double lon2)
        {
            double lat1Rad = DegreeToRadian(lat1);
            double lon1Rad = DegreeToRadian(lon1);
            double lat2Rad = DegreeToRadian(lat2);
            double lon2Rad = DegreeToRadian(lon2);

            double dLon = lon2Rad - lon1Rad;

            double y = Math.Sin(dLon) * Math.Cos(lat2Rad);
            double x = Math.Cos(lat1Rad) * Math.Sin(lat2Rad) -
                      Math.Sin(lat1Rad) * Math.Cos(lat2Rad) * Math.Cos(dLon);

            double azimuthRad = Math.Atan2(y, x);
            double azimuth = RadianToDegree(azimuthRad);

            return (azimuth + 360) % 360;
        }

        public static double DegreeToRadian(double deg) => deg * Math.PI / 180.0;
        public static double RadianToDegree(double rad) => rad * 180.0 / Math.PI;

        private static double NormalizeAngle(double angle)
        {
            // 将角度标准化到[-180, 180]范围
            angle %= 360;
            if (angle > 180) angle -= 360;
            if (angle < -180) angle += 360;
            return angle;
        }
    }

    public class TargetLocator
    {
        private const double GroundTolerance = 1e-10;
        private const double TerrainHeight = 0.0; // 默认地形高度（可替换为实际地形数据）

        /// <summary>
        /// 计算图像中目标点的地面坐标
        /// </summary>
        public static (double Lon, double Lat) LocateTarget(
            double droneLon, double droneLat, double droneAlt,
            double yaw, double pitch, double roll,
            CameraModel camera,
            double px, double py)
        {
            // 1. 获取高精度旋转矩阵
            double[,] R = GeodeticUtils.CreateRotationMatrix(yaw, pitch, roll);

            // 2. 将图像坐标转换为相机坐标系方向向量
            var (camX, camY, camZ) = camera.ImageToCameraVector(px, py);

            // 3. 转换到ENU坐标系
            double[] enuDirection = {
                R[0,0] * camX + R[0,1] * camY + R[0,2] * camZ,
                R[1,0] * camX + R[1,1] * camY + R[1,2] * camZ,
                R[2,0] * camX + R[2,1] * camY + R[2,2] * camZ
            };

            // 4. 归一化方向向量
            double norm = Math.Sqrt(
                enuDirection[0] * enuDirection[0] +
                enuDirection[1] * enuDirection[1] +
                enuDirection[2] * enuDirection[2]
            );

            enuDirection[0] /= norm;
            enuDirection[1] /= norm;
            enuDirection[2] /= norm;

            // 5. 处理特殊姿态
            if (Math.Abs(enuDirection[2]) < GroundTolerance)
            {
                // 水平指向时返回无人机正下方位置
                return (droneLon, droneLat);
            }

            // 6. 计算与地面交点（考虑地形高度）
            double heightAboveTerrain = droneAlt - TerrainHeight;
            if (heightAboveTerrain <= 0)
                throw new InvalidOperationException("Drone is below terrain level");

            double t = -heightAboveTerrain / enuDirection[2];
            double east = enuDirection[0] * t;
            double north = enuDirection[1] * t;

            // 7. 高精度转换到地理坐标
            (double ecefX, double ecefY, double ecefZ) =
                GeodeticUtils.EnuToEcef(east, north, TerrainHeight, droneLat, droneLon, 0);

            (double lat, double lon, double _) = GeodeticUtils.EcefToGeodetic(ecefX, ecefY, ecefZ);

            return (lon, lat);
        }

        /// <summary>
        /// 计算目标区域中心点的经纬度
        /// </summary>
        public static (double Lon, double Lat) LocateTargetArea(
            double droneLon, double droneLat, double droneAlt,
            double yaw, double pitch, double roll,
            CameraModel camera,
            double x1, double y1, double x2, double y2)
        {
            // 计算目标区域中心点
            double centerX = (x1 + x2) / 2.0;
            double centerY = (y1 + y2) / 2.0;

            return LocateTarget(
                droneLon, droneLat, droneAlt,
                yaw, pitch, roll,
                camera, centerX, centerY);
        }

        /// <summary>
        /// 计算目标区域四个角点的经纬度
        /// </summary>
        public static ((double Lon, double Lat) TopLeft,
                       (double Lon, double Lat) TopRight,
                       (double Lon, double Lat) BottomRight,
                       (double Lon, double Lat) BottomLeft)
        LocateTargetAreaCorners(
            double droneLon, double droneLat, double droneAlt,
            double yaw, double pitch, double roll,
            CameraModel camera,
            double x1, double y1, double x2, double y2)
        {
            var topLeft = LocateTarget(droneLon, droneLat, droneAlt, yaw, pitch, roll, camera, x1, y1);
            var topRight = LocateTarget(droneLon, droneLat, droneAlt, yaw, pitch, roll, camera, x2, y1);
            var bottomRight = LocateTarget(droneLon, droneLat, droneAlt, yaw, pitch, roll, camera, x2, y2);
            var bottomLeft = LocateTarget(droneLon, droneLat, droneAlt, yaw, pitch, roll, camera, x1, y2);

            return (topLeft, topRight, bottomRight, bottomLeft);
        }
    }

}